package voloshyn.paliy.arduino;

public class Caterpillar2 {
    private static final int MIN_SPEED = 84;
    private static final int ANGLE_FOR_TURN = 20;
    private int leftSpeed = 255;
    private int rigthSpeed = 255;
    public static float speedKoef = 255 / 45;

    private Caterpillar2() {
    };

    private void setLeftSpeed(int speed) {
	if (speed >= -255 && speed <= 255) {
	    leftSpeed = speed;
	} else {
	    throw new UnsupportedOperationException();
	}
    }

    private void setRightSpeed(int speed) {
	if (speed >= -255 && speed <= 255) {
	    rigthSpeed = speed;
	} else {
	    throw new UnsupportedOperationException();
	}
    }

    public int getRightSpeed() {
	return rigthSpeed;
    }

    public int getLeftSpeed() {
	return leftSpeed;
    }

    private static int getQuarter(float x, float y) {
	if (x >= 0 && y >= 0) {
	    return 1;
	} else if (x <= 0 && y >= 0) {
	    return 2;
	} else if (x <= 0 && y <= 0) {
	    return 3;
	}
	return 4;
    }

    public static Caterpillar2 getCaterpillar(float x, float y) {
	Caterpillar2 caterpillar2 = new Caterpillar2();
	caterpillar2.setCaterpillarSpeed(getQuarter(x, y), caterpillar2, x, y);
	return caterpillar2;
    }

    private void setCaterpillarSpeed(int quarter, Caterpillar2 caterpillar2,
	    float x, float y) {
	int angle = getAngle(x, y);
	int absAngle = Math.abs(angle);
	if (absAngle <= ANGLE_FOR_TURN)// r
	{
	    caterpillar2.setLeftSpeed(255);
	    caterpillar2.setRightSpeed(-255);
	} else if (absAngle >= 180 - ANGLE_FOR_TURN)// l
	{
	    caterpillar2.setLeftSpeed(-255);
	    caterpillar2.setRightSpeed(255);
	} else if (angle >= ANGLE_FOR_TURN && angle <= 180 - ANGLE_FOR_TURN)// f
	{
//	    if(angle>=100 && angle<=80 )
//	    {
//		caterpillar2.setLeftSpeed(255);
//		caterpillar2.setRightSpeed(255);
//	    }
//	    else 
		if (angle <= 90) {
		caterpillar2.setLeftSpeed(255);
		caterpillar2.setRightSpeed(getVelocityByAngle(angle));
	    } else {
		caterpillar2.setRightSpeed(255);
		caterpillar2.setLeftSpeed(getVelocityByAngle(angle));
	    }
	} else if (angle <= -ANGLE_FOR_TURN && angle >= -180 + ANGLE_FOR_TURN)// b
	{
//	    if(angle>=-100 && angle<=-80 )
//	    {
//		caterpillar2.setLeftSpeed(-255);
//		caterpillar2.setRightSpeed(-255);
//	    }
//	    else 
		if (angle >= -90) {
		caterpillar2.setLeftSpeed(-255);
		caterpillar2.setRightSpeed(getVelocityByAngle(angle));
	    } else {
		caterpillar2.setRightSpeed(-255);
		caterpillar2.setLeftSpeed(getVelocityByAngle(angle));
	    }
	}
    }

    private int getVelocityByAngle(int angle) {
	boolean minus = angle < 0;
	int absAngle = java.lang.Math.abs(angle);
	int velocity = 0;
	double speed = 255 / (90);
	if (absAngle <= 90) {
	    velocity = (int) (absAngle * speed);
	  } else {
	      velocity = (int) ((180 - absAngle) * speed);
	}

	if (velocity < MIN_SPEED) {
	    velocity = MIN_SPEED;
	}
	if (velocity > 255) {
	    velocity = 255;
	}
	if (minus) {
	    velocity *= (-1);
	}

	return velocity;
    }

    private int getAngle(float x, float y) {
	return (int) (Math.atan2(y, x) * 180.0 / Math.PI);
    }

    @Override
    public String toString() {
	return "l:" + leftSpeed + "r:" + rigthSpeed;
    }

}
